#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h"

class PosCal : public rclcpp::Node
{
    private:
        rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
        void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
        {

        }
    public:
        PosCal(std::string name) : Node(name)
        {
            RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str());
            //订阅imu
            imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
                "imu/data_raw", 10,
                std::bind(&PosDataV3::imuCallback, this, std::placeholders::_1));
            // 创建一个订阅者L订阅话题
            command_subscribe_L = this->create_subscription<std_msgs::msg::Float32>(
                "esp_pro_l", 10, std::bind(&PosDataV3::vL_callback, this, std::placeholders::_1));
            // 创建一个订阅者R订阅话题
            command_subscribe_R = this->create_subscription<std_msgs::msg::Float32>(
                "esp_pro_r", 10, std::bind(&PosDataV3::vR_callback, this, std::placeholders::_1));
        }
};